/*
 * @Author       : Zou Kai
 * @Date         : 2024-11-12 14:16:50
 * @Last Modified by: Your Name
 * @Last Modified time: 
 * @Description  : 
 */


#include "common.h"
#include<opencv2/core/eigen.hpp>

Eigen::Matrix4d toTransfromMatrix(const Eigen::Quaterniond &r_quaternion,
                                  const Eigen::Vector3d &t_vec) {
    Eigen::Matrix4d transformMatrix = Eigen::Matrix4d::Identity();
    Eigen::Matrix3d rotation_matrix = r_quaternion.toRotationMatrix();
    // 设置旋转矩阵部分
    transformMatrix.block<3, 3>(0, 0) = r_quaternion.toRotationMatrix();
    // 设置平移矩阵部分
    transformMatrix.block<3, 1>(0, 3) = t_vec;
    return transformMatrix;
}

bool vec2D2Mat(const std::vector<std::vector<double>>& vec,
    cv::Mat& matrix){
    matrix = cv::Mat::zeros(cv::Size(vec[0].size(), vec.size()), CV_64F);
    for (size_t i = 0; i < vec.size(); i++) {
        for (size_t j = 0; j < vec[i].size(); j++)
        matrix.at<double>(i, j) = static_cast<float>(vec[i][j]);
    }
    return true;    
}

bool vec1D2Mat(const std::vector<double>& vec,cv::Mat& matrix,
            int direction){
  if (direction == 0) {
    matrix = cv::Mat::zeros(cv::Size(1, vec.size()), CV_64F);
    for (size_t i = 0; i < vec.size(); i++) {
      matrix.at<double>(i, 0) = static_cast<double>(vec[i]);
    }
  } else if (direction == 1) {
    matrix = cv::Mat::zeros(cv::Size(vec.size(), 1), CV_64F);
    for (size_t i = 0; i < vec.size(); i++) {
      matrix.at<double>(0, i) = static_cast<double>(vec[i]);
    }
  }
  return true;
}


// bool LoadIntrinsic(const std::string filename, std::vector<std::vector<double>>& K_vec ,
//                  std::vector<double>& D_vec){
//     if ((access(filename.c_str(), 0)) == -1) {
//         std::cout << "Failed to open settings file at: " << filename<<std::endl;;
//         return false;
//     }                
//     YAML::Node cam_config = YAML::LoadFile(filename);
//     D_vec =
//         cam_config["intrinsic"]["D"].as<std::vector<double>>();
//     K_vec =
//         cam_config["intrinsic"]["K"].as<std::vector<std::vector<double>>>();
    
//     // Eigen::Matrix3d intrinsic;
//     // intrinsic << K_vec[0][0], K_vec[0][1], K_vec[0][2],
//     //     K_vec[1][0], K_vec[1][1], K_vec[1][2],
//     //     K_vec[2][0], K_vec[2][1], K_vec[2][2];
//     // K = intrinsic;
//     return true;
// }


// bool LoadExtrinsic(const std::string filename, const std::string camera_name,
//                     Eigen::Matrix4d& transform_extrinsic){
//     std::cout<<filename<<std::endl;                    
//     YAML::Node lidar_config = YAML::LoadFile(filename);
//     if (!lidar_config) {
//     std::cout << "Failed to open settings file at: " << filename << std::endl;
//     return false;
//     }
//     Eigen::Quaterniond rotation;
//     Eigen::Vector3d t_vec;
//     // Eigen::Matrix4d transform_matrix;

//     YAML::Node lidarExtr_node = lidar_config["extrinsic"];
//     for (auto extr_it = lidarExtr_node.begin(); extr_it != lidarExtr_node.end();
//         extr_it++) 
//     {
//         std::string device_name = (extr_it->first).as<std::string>();
//         if(device_name != camera_name){
//             continue;
//         }
//         // std::vector<std::string>::iterator it = find(cam_names.begin(), cam_names.end(), bline_name);
//         // if (it == cam_names.end()){
//         //         std::cout << "not projector device name: " << device_name <<std::endl;
//         //         continue;  // Unknow device_name
//         // }
//         YAML::Node extr_params = extr_it->second;
//         t_vec[0] = extr_params["translation"]["x"].as<double>();
//         t_vec[1] = extr_params["translation"]["y"].as<double>();
//         t_vec[2] = extr_params["translation"]["z"].as<double>();
//         rotation.x() = extr_params["rotation"]["x"].as<double>();
//         rotation.y() = extr_params["rotation"]["y"].as<double>();
//         rotation.z() = extr_params["rotation"]["z"].as<double>();
//         rotation.w() = extr_params["rotation"]["w"].as<double>();
//         transform_extrinsic = toTransfromMatrix(rotation,t_vec);

//         // transform_extrinsic.emplace(device_name, transform_matrix.inverse());
//         // std::cout << "projector device name: " << device_name <<std::endl;
//         // std::cout << "translation: " << transform_matrix <<std::endl;
//     }
//     return true;
// };

// bool squareProject(const Eigen::Vector3d& center_point,const float cerr_loss,
//             const cv::Mat& camera_matrix,const cv::Mat& distortion_matrix,
//             std::vector<cv::Point2d>& square_vec){
//     std::vector<float> loss_vec({cerr_loss,-cerr_loss});
//     Eigen::Matrix3d K_matrix;
//     cv::cv2eigen(camera_matrix, K_matrix);
//     // std::vector<cv::Point2d> square_vec;
//     // std::cout<<center_point<<std::endl;
//     for(int i=0;i<2;i++){
//         double new_x = center_point[0] + loss_vec[i];
//         for(int j=0;j<2;j++){
//             double new_y = center_point[1] + loss_vec[j];
//             for(int k=0;k<2;k++){
//                 double new_z = center_point[2] + loss_vec[k];
//                 Eigen::Vector3d new_point(new_x,new_y,new_z);
//                 // std::cout<<"------------"<<std::endl;
//                 std::cout<<new_point<<std::endl;
//                 new_point = K_matrix * new_point;
//                 // std::cout<<new_point<<std::endl;
//                 cv::Point2d pt(new_point(0)/new_point(2), new_point(1)/new_point(2));
                
//                 // std::cout<<pt<<std::endl;
//                 std::vector<cv::Point2d> pt_vec;
//                 pt_vec.push_back(pt);
//                 // cv::Mat pt_mat  = cv::Mat(pt).reshape(1, 2);
//                 std::vector<cv::Point2d> undis_pt_mat;
//                 cv::fisheye::undistortPoints(pt_vec, undis_pt_mat, camera_matrix, distortion_matrix,cv::Mat(),camera_matrix);
//                 std::cout<<undis_pt_mat[0]<<std::endl;
                
//                 square_vec.push_back(undis_pt_mat[0]);
//             }
//         }
//     }
//     // std::cout<<square_vec[0]<<" "<<square_vec[1]<<" "<<square_vec[2]<<" "<<square_vec[3]<<std::endl;
//     return true;
// }